'''
Created on Mar 5, 2014

@author: Miri
'''

from math import atan2, degrees, pi
import math
import Project.Main as Main
import Project.Tracker.PointClass as PointClass

# Gets real coordinates and returns a normalized point class
def normalization ( x , y , workspaceSize):
    nx = (x/workspaceSize)*(2)-1
    ny = (y/workspaceSize)*(2)-1
    point = [ nx , ny ]
    return point

def pointNormalization(point):
    normPoint = PointClass.Point()
    normPoint.setX((point.x/Main.WORKSPACE_SIZE) * 2 - 1)
    normPoint.setY((point.y/Main.WORKSPACE_SIZE) * 2 - 1)
    return normPoint

# Return the distance between two points
def distance(p1, p2):
    #sign = (p1[0] - p2[0])/abs((p1[0] - p2[0])) to figure direction
    dist = math.sqrt(pow(p1[0] - p2[0],2)+ pow(p1[1] - p2[1],2))
    return  dist 

# Returns the angle between three points
def getAngleFromThreePoints(p1, p2, p3):
    p12 = norm(p1,p2)
    p23 = norm(p2,p3)
    p13 = norm(p1,p3)

    value = (pow(p12,2) + pow(p13,2) - pow(p23,2)) / (2 * p12 * p13)
    angleRad = math.acos(value)
    angle = math.degrees(angleRad)
    return 360 - angle #TODO: verify if correct direction or opposite
    
# Gets normalized point and returns one with real coordinates    
def normalizationBack ( point, workspaceSize):
    point[0] = ( (point[0]+1) * workspaceSize) / 2 
    point[1] =  ((point[1]+1) * workspaceSize)/ 2 
    return point

# Calculate the slope between two points
def findSlope(p1, p2):
    dx = p2.x - p1.x
    dy = p2.y - p1.y
    rads = atan2(-dy,dx)
    rads %= 2*pi
    degs = degrees(rads)


    return degs

# Smooth the contour of a polygon
def rectifyPolygon(pointsList, espilon):
    newPointList = [];
    i = 0;
    #for i in range(0, len(pointsList) - 1):
    
    
    pivotPoint = pointsList[0]
    newPointList.append(pivotPoint)
    degree = findSlope(pointsList[0], pointsList[1])
    
    for k in range(1, len(pointsList)):
        currDegree = findSlope(pointsList[0], pointsList[k])
        if (abs(degree - currDegree) > espilon):
            break
    i = k - 1
    
    while (i < len(pointsList) - 1):
        
        pivotPoint = pointsList[i]
        newPointList.append(pivotPoint)
        degree = findSlope(pointsList[i], pointsList[i+1])
        
        if ((i + 1) == (len(pointsList) - 1)):
            pivotPoint = pointsList[i + 1]
            newPointList.append(pivotPoint)
            break
        
        
        for k in range(i+1, len(pointsList)):
            currDegree = findSlope(pointsList[i], pointsList[k])
            if (abs(degree - currDegree) > espilon):
                break

        if (k == len(pointsList) - 1):
            break
        else:
            i = k - 1
    
    #Adds last 2 point to array        
    if (abs(degree - currDegree) > espilon):
        pivotPoint = pointsList[len(pointsList) - 2]
        newPointList.append(pivotPoint)
    pivotPoint = pointsList[len(pointsList) - 1]
    newPointList.append(pivotPoint)
        
    
        
    ####Check last point#######
    newListLength = len(newPointList)
    if (newListLength == 1):
        print('Error in rectifying polygon')
        return
    
    degreeFromLast = findSlope(newPointList[newListLength - 2], newPointList[newListLength - 1]) 
    degreeCenter = findSlope(newPointList[newListLength - 1], newPointList[0]) 
    degreeFromStart = findSlope(newPointList[0], newPointList[1]) 
        
    # Provide solution for worst case scenario when one edge is sampled by 4 points
    
    if (abs(degreeCenter - degreeFromStart) <= espilon) and  (abs(degreeCenter - degreeFromLast) > espilon):
        tempPoint = newPointList[0]
        newPointList.remove(tempPoint)
    elif  (abs(degreeCenter - degreeFromStart) > espilon) and  (abs(degreeCenter - degreeFromLast) <= espilon):
        tempPoint = newPointList[newListLength - 1]
        newPointList.remove(tempPoint)
    else:
        if (abs(degreeCenter - degreeFromStart) <= espilon):
            tempPoint = newPointList[newListLength - 1]
            newPointList.remove(tempPoint)
            newPointList[0] =  tempPoint
            newListLength = newListLength - 1
        
        if (abs(degreeCenter - degreeFromLast) <= espilon): 
            tempPoint = newPointList[newListLength - 1]
            newPointList.remove(tempPoint)
            newPointList[0] =  tempPoint

        
    return newPointList
                
        

def norm(p1, p2):
    dist = math.sqrt(pow(p1[0] - p2[0],2)+ pow(p1[1] - p2[1],2))
    return dist


def generateNewCommand(current, source, target):
    angleC = getAngleFromThreePoints(current, source, target)
    angleS = getAngleFromThreePoints(source, current, target)
    angleEnd = angleC - angleS
    dist = norm(current, target)
    outArg = [angleC, dist, angleEnd];
    return outArg


#TEST
p1 = [0,0]
p2 = [0,1]
p3 = [1,0]
print(generateNewCommand(p3, p1, p2))


